#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <termios.h>

#define KEYCODE_R 0x43
#define KEYCODE_L 0x44
#define KEYCODE_U 0x41
#define KEYCODE_D 0x42
#define KEYCODE_Q 0x71



void keyUpdateLoop(ros::Publisher pub)
{
	char c;
	int kfd = 0;
	bool dirty = false;
	double linear = 0;
	double angular = 0;
	int l_scale = 2;
	int a_scale = 2;
	struct termios cooked, raw;

	// get the console in raw mode
	tcgetattr(kfd, &cooked);
	memcpy(&raw, &cooked, sizeof(struct termios));
	raw.c_lflag &= ~ (ICANON | ECHO);

	// Setting a new line, then end of file
	raw.c_cc[VEOL] = 1;
	raw.c_cc[VEOF] = 2;
	tcsetattr(kfd, TCSANOW, &raw);

	puts("Reading from keyboard");
	puts("---------------------------");
	puts("Use arrow keys to move the tank.");
	puts("Use -/+ keys to dec/inc the linear speed.");
	puts("Use q/Q key to quit.");

	for(;;)
	{
		// get the next event from the keyboard
		if(read(kfd, &c, 1) < 0)
		{
			perror("read():");
			exit(-1);
		}

		linear = angular = 0;

		switch(c)
		{
			case KEYCODE_L:
				ROS_DEBUG("LEFT");
				angular = 1.0;
				dirty = true;
				break;

			case KEYCODE_R:
				ROS_DEBUG("RIGHT");
				angular = -1.0;
				dirty = true;
				break;

			case KEYCODE_U:
				ROS_DEBUG("UP");
				linear = 1.0;
				dirty = true;
				break;

			case KEYCODE_D:
				ROS_DEBUG("DOWN");
				linear = -1.0;
				dirty = true;
				break;
			case '=':
			case '+':
				l_scale += 1;
				printf("scale = %d\n", l_scale);
				break;

			case '-':
				l_scale -= 1;

				if(l_scale < 1)
				{
					l_scale = 1;
				}

				printf("scale = %d\n", l_scale);
				break;

			case 'q':
			case 'Q':
				tcsetattr(kfd, TCSANOW, &cooked);
				ros::shutdown();
				exit(0);
		}

		geometry_msgs::Twist twist;
		twist.angular.z = a_scale * angular;
		twist.linear.x = l_scale * linear;

		if(dirty == true)
		{
			pub.publish(twist);
			dirty = false;
		}
	}
}


int main(int argc, char** argv)
{
	ros::init(argc, argv, "tank_key");
	ros::NodeHandle nh;

	ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/tank/cmd_vel", 1);
	keyUpdateLoop(pub);

	return 0;
}

